from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy
import math

def main():
    rclpy.init()  # 初始化ROS 2
    nav = BasicNavigator()  # 创建导航指挥官实例

    # 定义初始位姿（机器人在地图中的初始位置）
    init_pose = PoseStamped()
    init_pose.header.frame_id = "map"  # 坐标系为地图坐标系
    init_pose.header.stamp = nav.get_clock().now().to_msg()  # 时间戳设为当前时间
    # 位置：地图原点 (0,0,0)
    init_pose.pose.position.x = 4.0
    init_pose.pose.position.y = 2.0
    init_pose.pose.position.z = 0.0

    # 计算向右旋转20度（绕Z轴旋转-20度）的四元数
    angle = math.radians(-20)  # 转换为弧度，负号表示向右旋转
    init_pose.pose.orientation.x = 0.0
    init_pose.pose.orientation.y = 0.0
    init_pose.pose.orientation.z = math.sin(angle / 2)
    init_pose.pose.orientation.w = math.cos(angle / 2)

    # 将初始位姿发送给Nav2（用于AMCL定位初始化）
    nav.setInitialPose(init_pose)

    # 等待Nav2导航栈完全启动（定位、规划等节点就绪）
    nav.waitUntilNav2Active()
    print("Nav2导航系统已激活,初始位姿设置完成!")

    # 保持节点运行（若后续无其他逻辑，可注释此行）
    # rclpy.spin(nav)

    rclpy.shutdown()  # 关闭ROS 2

if __name__ == '__main__':
    main()
